-- SET VARIABLES
port = 1
port2 = 1
motorPortL = 3
motorPortR = 1
targetHeading = 10
-- API functions, hemple design group.
function nxt.I2Cdata( devaddr )
  return nxt.I2CReadString( devaddr, 0x42 )
end
function nxt.I2CReadString( devaddr, regaddr )
  return string.char( devaddr, regaddr )
end
function waitI2C( port )
  while( 0 ~= nxt.I2CGetStatus( port ) ) do
  end
end
function setupI2C(a)
  nxt.InputSetType(a,11)
  nxt.I2CInitPins(a)
end
-- Our code
function sleep(ms)
	endtime = nxt.TimerRead() + ms
	while nxt.TimerRead() < endtime do
	end
end
function getIRData(port2)
	nxt.I2CSendData(port2, string.char(0x10, 0x42), 8)
	waitI2C(port2)
	string.byte(nxt.I2CRecvData(port2))
end
function getHeading(port)
	nxt.I2CSendData(port, nxt.I2Cdata(2), 4)
	waitI2C(port)
	data = nxt.I2CRecvData(port, 4)
	b1, b2, b3, b4 = string.byte(data, 1, 8) 
	if b4 == 1 then
		truevalue = b3 + 255
	else
		truevalue = b3
	end
	return truevalue
end
-- Driving functions
function steerForward()
	nxt.OutputSetSpeed(motorPortL, 32, -80)
	nxt.OutputSetSpeed(motorPortR, 32, -80)
end
function steerLeft()
	nxt.OutputSetSpeed(motorPortL, 32, 80)
	nxt.OutputSetSpeed(motorPortR, 32, -80)
end
function steerRight()
	nxt.OutputSetSpeed(motorPortL, 32, -80)
	nxt.OutputSetSpeed(motorPortR, 32, 80)
end
function allStop()
	nxt.OutputSetSpeed(motorPortL, 32, 0)
	nxt.OutputSetSpeed(motorPortR, 32, 0)
end

setupI2C(port2)


while 8 ~= nxt.ButtonRead() do
	=getIRData(port2)
	--[[normalisedHeading = getHeading(port) - targetHeading
	
	if normalisedHeading < 0 then
		normalisedHeading = normalisedHeading + 360
	end
	
	print("Normalised heading: "..normalisedHeading)
	
	if normalisedHeading > 355 or normalisedHeading < 5 then
		steerForward()		
	elseif normalisedHeading < 180 then
		steerLeft()
	elseif normalisedHeading > 180 then
		steerRight()
	end 
	sleep(500)]]--
	sleep(500)
end
allStop()
--